import math

class PIDController:
    def __init__(self, Kp, Ki, Kd, integral_limit):
        self.Kp = Kp
        self.Ki = Ki
        self.Kd = Kd
        self.prev_error = 0
        self.integral = 0
        self.integral_limit = integral_limit

    def calculate(self, error):
        self.integral += error
        if self.integral_limit is not None:
            if self.integral > self.integral_limit:
                self.integral = self.integral_limit
            elif self.integral < -self.integral_limit:
                self.integral = -self.integral_limit
        derivative = error - self.prev_error
        self.prev_error = error
        return self.Kp * error + self.Ki * self.integral + self.Kd * derivative

def navigate_to_target(current_heading, current_x, current_y, target_x, target_y,current_speed):
    # 计算目标航向
    target_x_ship = target_x - current_x
    target_y_ship = target_y - current_y

    # 计算目标航向误差
    heading_error = math.atan2(target_y_ship, target_x_ship) - math.radians(current_heading) + math.pi

    # 范围调整到 -π 到 π 之间
    heading_error = (heading_error + math.pi) % (2 * math.pi) - math.pi

    # PID控制器参数
    k= 0.1
    Kp_servo = 0.4 * k
    Ki_servo = 0.01 *k
    Kd_servo = 0.5 *k

    # 初始化PID控制器，并设置积分项限制
    integral_limit =3 * k # 根据需要设置积分项限制
    pid_servo = PIDController(Kp_servo, Ki_servo, Kd_servo, integral_limit)

    # 计算舵角
    servo_command = pid_servo.calculate(heading_error)
    k_1 = 0.1
    Kp_power = 0.6 * k_1
    Ki_power = 0.4 * k_1
    Kd_power = 0.4 * k_1

    # 初始化推进力PID控制器，并设置积分项限制
    pid_power = PIDController(Kp_power, Ki_power, Kd_power, 1)

    # 计算速度误差
    speed_error = 2 - current_speed

    # 计算推进力
    #power_command = pid_power.calculate(speed_error)
    power_command = 1
    # print('当前艏向和速度：',current_heading,current_speed)
    # print('命令：',servo_command, power_command)
    return servo_command, power_command
